﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using RVO;

namespace AIPrakt05
{
    public class AgentSimulator
    {
        IList<Vector2> goalsList;
        bool[] goalReached;
        int goalReachedCounter;
        public Simulator Sim { get; set; }
        public event Action<Simulator> SimulationChangedEvent;

        public AgentSimulator()
        {
            goalsList = new List<Vector2>();

            // Create a new simulator instance.
            Sim = Simulator.Instance;

            // Set up the scenario.
            SetupScenario(Sim);          
        }

        public void RunSimulation()
        {
            UpdateConsole(Sim);
            SimulationChangedEvent(Sim);
            while (true)
            {
                SetPreferredVelocities(Sim);
                Sim.doStep();
                //updateConsole(Sim);
                SimulationChangedEvent(Sim);
            }
        }


        private bool ReachedGoal()
        {
            // Check whether all agents have arrived at their goals.
            for (int i = 0; i < Sim.getNumAgents(); ++i)
            {
                if (RVOMath.absSq(goalsList[i] - Sim.getAgentPosition(i)) > Sim.getAgentRadius(i) * Sim.getAgentRadius(i))
                {
                    // Agent is further away from its goal than one radius.
                    return false;
                }
            }
            return true;
        }

        private void SetPreferredVelocities(Simulator sim)
        {
            Random random = new Random();

            for (int i = 0; i < sim.getNumAgents(); ++i) {
                if (goalReachedCounter >= 74 && i >= 51)
                {
                    if (Math.Sqrt(RVOMath.absSq(goalsList[i + 24] - sim.getAgentPosition(i))) < sim.getAgentRadius(i) * 0.1)
                    {
                        sim.setAgentPrefVelocity(i, new Vector2(0.0f, 0.0f));
                    }
                    else
                    {
                        sim.setAgentPrefVelocity(i, RVOMath.normalize(goalsList[i + 24] - sim.getAgentPosition(i)));
                    }
                } else if (Math.Sqrt(RVOMath.absSq(goalsList[i] - sim.getAgentPosition(i))) < sim.getAgentRadius(i) * 0.1)
                {
                    if (goalReached[i] == false)
                    {
                        goalReached[i] = true;
                        goalReachedCounter++;
                    }
                    sim.setAgentPrefVelocity(i, new Vector2(0.0f, 0.0f));
                } else {
                    sim.setAgentPrefVelocity(i, RVOMath.normalize(goalsList[i] - sim.getAgentPosition(i)));
                }
            }
        }

        private void SetupScenario(Simulator sim)
        {
            sim.setTimeStep(0.05f);

            sim.SetNumWorkers(5);

            float neighborDist = 55.0f;
            int maxNeighbors = 20;
            float timeHorizon = 60.0f;
            float timeHorizonObst = 1.0f;
            float radius = 8.0f;
            float maxSpeed = 2.0f;
            Vector2 velocity = new Vector2(0f, 0f);
            sim.setAgentDefaults(neighborDist, maxNeighbors, timeHorizon, timeHorizonObst, radius, maxSpeed, velocity);

            Random random = new Random();
            int i=0;
            while(i < 17)
            {
                sim.addAgent(new Vector2(750, random.Next(-50, 450)));
                i++;
            }
            while (i < 38)
            {
                sim.addAgent(new Vector2(-50, random.Next(-50, 450)));
                i++;
            }
            while (i < 51)
            {
                sim.addAgent(new Vector2(random.Next(-50, 750), 450));
                i++;
            }
            while (i < 75)
            {
                sim.addAgent(new Vector2(random.Next(-50, 750), -50));
                i++;
            }

            goalReached = new bool[75];

        //H:

            goalsList.Add(new Vector2(150, 40));
            goalsList.Add(new Vector2(150, 60));
            goalsList.Add(new Vector2(150, 80));
            goalsList.Add(new Vector2(150, 100));
            goalsList.Add(new Vector2(150, 120));
            goalsList.Add(new Vector2(150, 140));
            goalsList.Add(new Vector2(150, 160));

            goalsList.Add(new Vector2(170, 100));
            goalsList.Add(new Vector2(190, 100));
            goalsList.Add(new Vector2(210, 100));

            goalsList.Add(new Vector2(230, 40));
            goalsList.Add(new Vector2(230, 60));
            goalsList.Add(new Vector2(230, 80));
            goalsList.Add(new Vector2(230, 100));
            goalsList.Add(new Vector2(230, 120));
            goalsList.Add(new Vector2(230, 140));
            goalsList.Add(new Vector2(230, 160));

        //W:

            goalsList.Add(new Vector2(430, 40));
            goalsList.Add(new Vector2(435, 60));
            goalsList.Add(new Vector2(440, 80));
            goalsList.Add(new Vector2(445, 100));
            goalsList.Add(new Vector2(450, 120));
            goalsList.Add(new Vector2(455, 140));
            goalsList.Add(new Vector2(465, 160));
            goalsList.Add(new Vector2(475, 140));
            goalsList.Add(new Vector2(480, 120));
            goalsList.Add(new Vector2(485, 100));
            goalsList.Add(new Vector2(495, 80));
            goalsList.Add(new Vector2(505, 100));
            goalsList.Add(new Vector2(510, 120));
            goalsList.Add(new Vector2(515, 140));
            goalsList.Add(new Vector2(525, 160));
            goalsList.Add(new Vector2(535, 140));
            goalsList.Add(new Vector2(540, 120));
            goalsList.Add(new Vector2(545, 100));
            goalsList.Add(new Vector2(550, 80));
            goalsList.Add(new Vector2(555, 60));
            goalsList.Add(new Vector2(560, 40));
            
            //T:

            goalsList.Add(new Vector2(270, 40));
            goalsList.Add(new Vector2(290, 40));
            goalsList.Add(new Vector2(310, 40));
            goalsList.Add(new Vector2(330, 40));
            goalsList.Add(new Vector2(350, 40));
            goalsList.Add(new Vector2(370, 40));
            goalsList.Add(new Vector2(390, 40));

            goalsList.Add(new Vector2(330, 60));
            goalsList.Add(new Vector2(330, 80));
            goalsList.Add(new Vector2(330, 100));
            goalsList.Add(new Vector2(330, 120));
            goalsList.Add(new Vector2(330, 140));
            goalsList.Add(new Vector2(330, 160));

        //Unterstrich:

            for (int j = 130; j <= 590; j += 20)
            {
                goalsList.Add(new Vector2(j, 190));
            }

        //A:

            goalsList.Add(new Vector2(270, 230));
            goalsList.Add(new Vector2(290, 230));
            goalsList.Add(new Vector2(310, 230));
            goalsList.Add(new Vector2(330, 230));
            goalsList.Add(new Vector2(350, 230));

            goalsList.Add(new Vector2(290, 270));
            goalsList.Add(new Vector2(310, 270));
            goalsList.Add(new Vector2(330, 270));

            goalsList.Add(new Vector2(270, 250));
            goalsList.Add(new Vector2(270, 270));
            goalsList.Add(new Vector2(270, 290));
            goalsList.Add(new Vector2(270, 310));
            goalsList.Add(new Vector2(270, 330));

            goalsList.Add(new Vector2(350, 250));
            goalsList.Add(new Vector2(350, 270));
            goalsList.Add(new Vector2(350, 290));
            goalsList.Add(new Vector2(350, 310));
            goalsList.Add(new Vector2(350, 330));

        //I:

            goalsList.Add(new Vector2(390, 230));
            goalsList.Add(new Vector2(390, 250));
            goalsList.Add(new Vector2(390, 270));
            goalsList.Add(new Vector2(390, 290));
            goalsList.Add(new Vector2(390, 310));
            goalsList.Add(new Vector2(390, 330));

            //sim.processObstacles();
        }

        private void UpdateConsole(Simulator sim)
        {
            // Output the current global time.
            Console.WriteLine("Time: " + sim.getGlobalTime());

            Console.WriteLine("Agents:");
            // Output the position for all the agents.
            for (int i = 0; i < sim.getNumAgents(); ++i)
            {
                Console.WriteLine("Agent " + i + ": Pos = " + sim.getAgentPosition(i) + "Velocity = " + sim.getAgentVelocity(i));
            }
            Console.WriteLine("\n");
        }
    }
}
